#!/usr/bin/env python
# -*- coding:utf-8 -*-
import rospy
import tf
from nav_msgs.msg import Odometry
from sensor_msgs.msg import LaserScan
from sensor_msgs.msg import Image
import cv2
import cv_bridge
from camera_info_manager import CameraInfoManager,CameraInfo


class tfTrans:
    def __init__(self):
        self.bridge = cv_bridge.CvBridge()

    def imageCallback(self,img):
        cv_img = self.bridge.imgmsg_to_cv2(img, "bgr8")
        equal_img = cv_img[0:480,0:640]
        img_msg = self.bridge.cv2_to_imgmsg(equal_img, "bgr8")
        right_img_msg  = self.bridge.cv2_to_imgmsg(cv_img[0:480,640:1280], "bgr8")
        img_msg.header = img.header
        right_img_msg.header = img.header
        self.left_camera_info.header = img.header
        self.right_camera_info.header = img.header
        self.pub.publish(img_msg)
        self.pubr.publish(right_img_msg)
        self.left_info_pub.publish(self.left_camera_info)
        self.right_info_pub.publish(self.right_camera_info)
    
    def listener(self):
        rospy.init_node('camera_splitter', anonymous=False)
        rospy.Subscriber('/cam/image_raw', Image, self.imageCallback)
        self.pub = rospy.Publisher('/my/left/image_raw',Image,queue_size=1)
        self.pubr = rospy.Publisher('/my/right/image_raw',Image,queue_size=1)
        
        self.left_ci = CameraInfoManager(cname="leftCam", namespace='my/left')
        self.right_ci = CameraInfoManager(cname='rightCam', namespace='my/right')

        self.left_ci.loadCameraInfo()
        if not self.left_ci.isCalibrated():
            rospy.logwarn('Camera is not calibrated, please supply a valid camera_info_url parameter!')

        self.right_ci.loadCameraInfo()
        if not self.right_ci.isCalibrated():
            rospy.logwarn('Camera is not calibrated, please supply a valid camera_info_url parameter!')
        

        self.left_info_pub = rospy.Publisher("/my/left/camera_info", CameraInfo, queue_size=10)
        self.right_info_pub = rospy.Publisher("/my/right/camera_info", CameraInfo, queue_size=10)
        self.left_camera_info = self.left_ci.getCameraInfo()
        self.right_camera_info = self.right_ci.getCameraInfo()
        self.right_camera_info.header.frame_id="right_camera"
        rospy.spin()
 
if __name__ == '__main__':
    trans = tfTrans()
    trans.listener()   
